

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

using namespace std;

static void getAllFilesInDirs(std::string path, std::vector<std::string> &filePath)
{
    DIR *pDir;
    struct dirent *ptr;
    if (!(pDir = opendir(path.c_str())))
        return;
    while ((ptr = readdir(pDir)) != 0)
    {
        filePath.push_back(ptr->d_name);
    }
    closedir(pDir);
}

void checkPcdPath(std::vector<std::string> &filePath)
{
    std::vector<std::string> output;
    for (auto &path : filePath)
    {
        if (path.size() <= 4)
            continue;
        std::string pcd = path.substr(path.size() - 3, path.size());
        if (pcd == "pcd")
        {
            output.push_back(path); //检查格式
        }
    }
    std::sort(output.begin(), output.end(), [](std::string &a, std::string &b)
              { return std::atoi(a.substr(0, a.size() - 4).c_str()) < std::atoi(b.substr(0, b.size() - 4).c_str()); });
    filePath = output;
}

int main(int argc, char **argv)
{
    std::string path = "/home/zwh/work_space/18xx/adt-1862_for_calib/TEST_DATA/vlp16";
    std::string pos = "/front/";
    std::string pos2 = "/left/";
    std::vector<std::string> filePaths;
    getAllFilesInDirs(path + pos, filePaths);
    checkPcdPath(filePaths);

    Eigen::Affine3f transform0;
    transform0 = Eigen::Affine3f::Identity();
    transform0.translation() << 1.0, 2.0, 0.5;
    transform0.rotate(Eigen::AngleAxisf(30.0 / 180.0 * M_PI, Eigen::Vector3f::UnitZ()));
    transform0.rotate(Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitY()));
    transform0.rotate(Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitX()));

    for (const auto &filePath : filePaths)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
        if (pcl::io::loadPCDFile<pcl::PointXYZI>(path + pos + filePath, *frames) == -1) //*Žò¿ªµãÔÆÎÄŒþ
        {
            PCL_ERROR("Couldn't read file.pcd\n");
            return (-1);
        }

        cout << "ori" << endl;
        for (size_t i = 0; i < 5; i++)
        {
            cout << frames->points[i].x << " " << frames->points[i].y << " " << frames->points[i].z << endl;
        }
        pcl::transformPointCloud(*frames, *frames, transform0);
        cout << "tar" << endl;
        for (size_t i = 0; i < 5; i++)
        {
            cout << frames->points[i].x << " " << frames->points[i].y << " " << frames->points[i].z << endl;
        }
        // 生成pcd
        cout << "生成 :" << path + pos2 + filePath << endl;
        pcl::PCDWriter writer;
        writer.write(path + pos2 + filePath, *frames);
    }

    return 0;
}
